
#include "util.h"
#include "rpi_camera_control.h"

#include "bcm_host.h"
#include "interface/mmal/mmal.h"
#include "interface/mmal/util/mmal_default_components.h"
#include "interface/mmal/util/mmal_connection.h"
#include "interface/mmal/util/mmal_util_params.h"
#include "interface/mmal/mmal_logging.h"

#include <string.h>

// ============================================================

#define VIDEO_OUTPUT_BUFFERS_NUM 3
#define MMAL_PARAMETER_CAMERA_INFO_MAX_STR_LEN 32

typedef struct rpi_camera_param
{
    int sharpness;    /// -100 to 100
    int contrast;    /// -100 to 100
    int brightness;    ///  0 to 100
    int saturation;    ///  -100 to 100
    int ISO;    ///  TODO : what range?
    int videoStabilisation;    /// 0 or 1 (false or true)
    int exposureCompensation;  /// -10 to +10 ?
    MMAL_PARAM_EXPOSUREMODE_T exposureMode;
    MMAL_PARAM_EXPOSUREMETERINGMODE_T exposureMeterMode;
    MMAL_PARAM_AWBMODE_T awbMode;
    MMAL_PARAM_IMAGEFX_T imageEffect;
    MMAL_PARAMETER_IMAGEFX_PARAMETERS_T imageEffectsParameters;
    MMAL_PARAM_COLOURFX_T colourEffects;
    MMAL_PARAM_FLICKERAVOID_T flickerAvoidMode;
    int rotation;    /// 0-359
    int hflip;    /// 0 or 1
    int vflip;    /// 0 or 1
    PARAM_FLOAT_RECT_T  roi;    /// region of interest to use on the sensor. Normalised [0,1] values in the rect
    int shutter_speed;    /// 0 = auto, otherwise the shutter speed in ms
    float awb_gains_r;    /// AWB red gain
    float awb_gains_b;    /// AWB blue gain
    MMAL_PARAMETER_DRC_STRENGTH_T drc_level;  // Strength of Dynamic Range compression to apply
    MMAL_BOOL_T stats_pass;    /// Stills capture statistics pass on/off
    int enable_annotate;    /// Flag to enable the annotate, 0 = disabled, otherwise a bitmask of what needs to be displayed
    char annotate_string[MMAL_CAMERA_ANNOTATE_MAX_TEXT_LEN_V2]; /// String to use for annotate - overrides certain bitmask settings
    int annotate_text_size;    // Text size for annotation
    int annotate_text_colour;  // Text colour for annotation
    int annotate_bg_colour;    // Background colour for annotation
    unsigned int annotate_justify;
    unsigned int annotate_x;
    unsigned int annotate_y;
    int focus_window;
    MMAL_PARAMETER_STEREOSCOPIC_MODE_T stereo_mode;
    float analog_gain;    // Analog gain
    float digital_gain;    // Digital gain
    int settings;

    int cameraNum;    /// Camera number
    char camera_name[MMAL_PARAMETER_CAMERA_INFO_MAX_STR_LEN]; // Name of the camera sensor
    int sensor_mode;    /// Sensor mode. 0=auto. Check docs/forum for modes selected by other values.

    int width;    /// Requested width of image
    int height;    /// requested height of image
    int framerate;
} rpi_camera_param_t;

typedef struct rpi_camera
{
    rpi_camera_param_t params;
    MMAL_COMPONENT_T *camera;
} rpi_camera_t;

static void rpi_camera_param_default(rpi_camera_param_t *params, int width, int height)
{
    params->sharpness = 0;
    params->contrast = 0;
    params->brightness = 50;
    params->saturation = 0;
    params->ISO = 0;    // 0 = auto
    params->videoStabilisation = 0;
    params->exposureCompensation = 0;
    params->exposureMode = MMAL_PARAM_EXPOSUREMODE_AUTO;
    params->flickerAvoidMode = MMAL_PARAM_FLICKERAVOID_OFF;
    params->exposureMeterMode = MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE;
    params->awbMode = MMAL_PARAM_AWBMODE_AUTO;
    params->imageEffect = MMAL_PARAM_IMAGEFX_NONE;
    params->colourEffects.enable = 0;
    params->colourEffects.u = 128;
    params->colourEffects.v = 128;
    params->rotation = 0;
    params->hflip = params->vflip = 0;
    params->roi.x = params->roi.y = 0.0;
    params->roi.w = params->roi.h = 1.0;
    params->shutter_speed = 0;    // 0 = auto
    params->awb_gains_r = 0;    // Only have any function if AWB OFF is used.
    params->awb_gains_b = 0;
    params->drc_level = MMAL_PARAMETER_DRC_STRENGTH_OFF;
    params->stats_pass = MMAL_FALSE;
    params->enable_annotate = 0;
    params->annotate_string[0] = '\0';
    params->annotate_text_size = 0;	//Use firmware default
    params->annotate_text_colour = -1;    //Use firmware default
    params->annotate_bg_colour = -1;    //Use firmware default
    params->stereo_mode.mode = MMAL_STEREOSCOPIC_MODE_NONE;
    params->stereo_mode.decimate = MMAL_FALSE;
    params->stereo_mode.swap_eyes = MMAL_FALSE;

    params->cameraNum = 0;
    strncpy(params->camera_name, "OV5647", MMAL_PARAMETER_CAMERA_INFO_MAX_STR_LEN);
    params->sensor_mode = 0;

    params->width = width;
    params->height = height;
    params->framerate = 30;
}

static void default_camera_control_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
{
    if (buffer->cmd == MMAL_EVENT_PARAMETER_CHANGED)
    {
        MMAL_EVENT_PARAMETER_CHANGED_T *param = (MMAL_EVENT_PARAMETER_CHANGED_T *)buffer->data;
        switch (param->hdr.id)
        {
            case MMAL_PARAMETER_CAMERA_SETTINGS:
            {
                MMAL_PARAMETER_CAMERA_SETTINGS_T *settings = (MMAL_PARAMETER_CAMERA_SETTINGS_T*)param;
                log_error("Exposure now %u, analog gain %u/%u, digital gain %u/%u",
                    settings->exposure,
                    settings->analog_gain.num, settings->analog_gain.den,
                    settings->digital_gain.num, settings->digital_gain.den);
                log_error("AWB R=%u/%u, B=%u/%u",
                    settings->awb_red_gain.num, settings->awb_red_gain.den,
                    settings->awb_blue_gain.num, settings->awb_blue_gain.den);
                break;
            }
        }
    }
    else if (buffer->cmd == MMAL_EVENT_ERROR)
    {
        log_error("No data received from sensor. Check all connections, including the Sunny one on the camera board");
    }
    else
    {
        log_error("Received unexpected camera control callback event, 0x%08x", buffer->cmd);
    }
    mmal_buffer_header_release(buffer);
}

static int rpi_camera_control_set_all_parameters(MMAL_COMPONENT_T *camera, const rpi_camera_param_t *params)
{
    int result;

    result = raspicamcontrol_set_saturation(camera, params->saturation);
    result += raspicamcontrol_set_sharpness(camera, params->sharpness);
    result += raspicamcontrol_set_contrast(camera, params->contrast);
    result += raspicamcontrol_set_brightness(camera, params->brightness);
    result += raspicamcontrol_set_ISO(camera, params->ISO);
    result += raspicamcontrol_set_video_stabilisation(camera, params->videoStabilisation);
    result += raspicamcontrol_set_exposure_compensation(camera, params->exposureCompensation);
    result += raspicamcontrol_set_exposure_mode(camera, params->exposureMode);
    result += raspicamcontrol_set_flicker_avoid_mode(camera, params->flickerAvoidMode);
    result += raspicamcontrol_set_metering_mode(camera, params->exposureMeterMode);
    result += raspicamcontrol_set_awb_mode(camera, params->awbMode);
    result += raspicamcontrol_set_awb_gains(camera, params->awb_gains_r, params->awb_gains_b);
    result += raspicamcontrol_set_imageFX(camera, params->imageEffect);
    result += raspicamcontrol_set_colourFX(camera, &params->colourEffects);
    //result += raspicamcontrol_set_thumbnail_parameters(camera, &params->thumbnailConfig);  TODO Not working for some reason
    result += raspicamcontrol_set_rotation(camera, params->rotation);
    result += raspicamcontrol_set_flips(camera, params->hflip, params->vflip);
    result += raspicamcontrol_set_ROI(camera, params->roi);
    result += raspicamcontrol_set_shutter_speed(camera, params->shutter_speed);
    result += raspicamcontrol_set_DRC(camera, params->drc_level);
    result += raspicamcontrol_set_stats_pass(camera, params->stats_pass);
    //result += raspicamcontrol_set_gains(camera, params->analog_gain, params->digital_gain);
    result += raspicamcontrol_set_focus_window(camera, params->focus_window);

    if (params->settings)
    {
        MMAL_PARAMETER_CHANGE_EVENT_REQUEST_T change_event_request = {
            {MMAL_PARAMETER_CHANGE_EVENT_REQUEST, sizeof(MMAL_PARAMETER_CHANGE_EVENT_REQUEST_T)},
            MMAL_PARAMETER_CAMERA_SETTINGS, 1
        };

        MMAL_STATUS_T status = mmal_port_parameter_set(camera->control, &change_event_request.hdr);
        if ( status != MMAL_SUCCESS )
        {
            log_error("No camera settings events");
        }

        result += status;
    }

    return result;
}

static MMAL_STATUS_T create_camera_component(rpi_camera_t *rpi_camera)
{
    MMAL_STATUS_T status;

    MMAL_COMPONENT_T *camera = NULL;
    MMAL_ES_FORMAT_T *format;
    MMAL_PORT_T *preview_port = NULL, *video_port = NULL, *still_port = NULL;

    /* Create the component */
    status = mmal_component_create(MMAL_COMPONENT_DEFAULT_CAMERA, &camera);
    if (status != MMAL_SUCCESS)
    {
        log_error("Failed to create camera component");
        goto error;
    }

    status = raspicamcontrol_set_stereo_mode(camera->output[0], &rpi_camera->params.stereo_mode);
    status += raspicamcontrol_set_stereo_mode(camera->output[1], &rpi_camera->params.stereo_mode);
    status += raspicamcontrol_set_stereo_mode(camera->output[2], &rpi_camera->params.stereo_mode);
    if (status != MMAL_SUCCESS)
    {
        log_error("Could not set stereo mode : error %d", status);
        goto error;
    }

    MMAL_PARAMETER_INT32_T camera_num = {{MMAL_PARAMETER_CAMERA_NUM, sizeof(camera_num)}, rpi_camera->params.cameraNum};
    status = mmal_port_parameter_set(camera->control, &camera_num.hdr);
    if (status != MMAL_SUCCESS)
    {
        log_error("Could not select camera : error %d", status);
        goto error;
    }

    if (!camera->output_num)
    {
        status = MMAL_ENOSYS;
        log_error("Camera doesn't have output ports");
        goto error;
    }

    status = mmal_port_parameter_set_uint32(camera->control, MMAL_PARAMETER_CAMERA_CUSTOM_SENSOR_CONFIG, rpi_camera->params.sensor_mode);
    if (status != MMAL_SUCCESS)
    {
        log_error("Could not set sensor mode : error %d", status);
        goto error;
    }

    preview_port = camera->output[0];
    video_port = camera->output[1];
    still_port = camera->output[2];

    // Enable the camera, and tell it its control callback function
    status = mmal_port_enable(camera->control, default_camera_control_callback);
    if (status != MMAL_SUCCESS)
    {
        log_error("Unable to enable control port : error %d", status);
        goto error;
    }

    //  set up the camera configuration
    {
        MMAL_PARAMETER_CAMERA_CONFIG_T cam_config = {
            { MMAL_PARAMETER_CAMERA_CONFIG, sizeof(cam_config) },
            .max_stills_w = rpi_camera->params.width,
            .max_stills_h = rpi_camera->params.height,
            .stills_yuv422 = 0,
            .one_shot_stills = 0,
            .max_preview_video_w = rpi_camera->params.width,
            .max_preview_video_h = rpi_camera->params.height,
            .num_preview_video_frames = 3,
            .stills_capture_circular_buffer_height = 0,
            .fast_preview_resume = 0,
            .use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RAW_STC
        };
        mmal_port_parameter_set(camera->control, &cam_config.hdr);
    }

    if (rpi_camera->params.shutter_speed > 6000000)
    {
        MMAL_PARAMETER_FPS_RANGE_T fps_range = {
            {MMAL_PARAMETER_FPS_RANGE, sizeof(fps_range)},
            {5, 1000 }, {166, 1000}
        };
        mmal_port_parameter_set(preview_port, &fps_range.hdr);
    }
    else if(rpi_camera->params.shutter_speed > 1000000)
    {
        MMAL_PARAMETER_FPS_RANGE_T fps_range = {
            {MMAL_PARAMETER_FPS_RANGE, sizeof(fps_range)},
            { 166, 1000 }, {999, 1000}
        };
        mmal_port_parameter_set(preview_port, &fps_range.hdr);
    }

    //enable dynamic framerate if necessary
    if (rpi_camera->params.shutter_speed)
    {
        if (rpi_camera->params.framerate > (1000000.0/rpi_camera->params.shutter_speed))
        {
            rpi_camera->params.framerate = 0;
        }
    }

    // Now set up the port formats
    format = preview_port->format;
    format->encoding = MMAL_ENCODING_OPAQUE;
    format->encoding_variant = MMAL_ENCODING_NV12;
    format->es->video.width = VCOS_ALIGN_UP(rpi_camera->params.width, 32);
    format->es->video.height = VCOS_ALIGN_UP(rpi_camera->params.height, 16);
    format->es->video.crop.x = 0;
    format->es->video.crop.y = 0;
    format->es->video.crop.width = rpi_camera->params.width;
    format->es->video.crop.height = rpi_camera->params.height;
    format->es->video.frame_rate.num =  rpi_camera->params.framerate;
    format->es->video.frame_rate.den = 1;
    status = mmal_port_format_commit(preview_port);
    if (status != MMAL_SUCCESS)
    {
        log_error("camera viewfinder format couldn't be set");
        goto error;
    }

    // ==================================================

    // Set the encode format on the video  port
    if(rpi_camera->params.shutter_speed > 6000000)
    {
        MMAL_PARAMETER_FPS_RANGE_T fps_range = {
            {MMAL_PARAMETER_FPS_RANGE, sizeof(fps_range)},
            { 5, 1000 }, {166, 1000}
        };
        mmal_port_parameter_set(video_port, &fps_range.hdr);
    }
    else if(rpi_camera->params.shutter_speed > 1000000)
    {
        MMAL_PARAMETER_FPS_RANGE_T fps_range = {
            {MMAL_PARAMETER_FPS_RANGE, sizeof(fps_range)},
            { 167, 1000 }, {999, 1000}
        };
        mmal_port_parameter_set(video_port, &fps_range.hdr);
    }

    format = video_port->format;
    format->encoding_variant = MMAL_ENCODING_NV12;
    format->es->video.width = VCOS_ALIGN_UP(rpi_camera->params.width, 32);
    format->es->video.height = VCOS_ALIGN_UP(rpi_camera->params.height, 16);
    format->es->video.crop.x = 0;
    format->es->video.crop.y = 0;
    format->es->video.crop.width = rpi_camera->params.width;
    format->es->video.crop.height = rpi_camera->params.height;
    format->es->video.frame_rate.num =  rpi_camera->params.framerate;
    format->es->video.frame_rate.den = 1;
    status = mmal_port_format_commit(video_port);
    if (status != MMAL_SUCCESS)
    {
        log_error("camera video format couldn't be set");
        goto error;
    }

    // Ensure there are enough buffers to avoid dropping frames
    if (video_port->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM) {
        video_port->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM;
    }

    // ==================================================

    // Set the encode format on the still  port

    format = still_port->format;
    format->encoding = MMAL_ENCODING_OPAQUE;
    format->encoding_variant = MMAL_ENCODING_NV12;
    format->es->video.width = VCOS_ALIGN_UP(rpi_camera->params.width, 32);
    format->es->video.height = VCOS_ALIGN_UP(rpi_camera->params.height, 16);
    format->es->video.crop.x = 0;
    format->es->video.crop.y = 0;
    format->es->video.crop.width = rpi_camera->params.width;
    format->es->video.crop.height = rpi_camera->params.height;
    format->es->video.frame_rate.num = 0;
    format->es->video.frame_rate.den = 1;
    status = mmal_port_format_commit(still_port);
    if (status != MMAL_SUCCESS)
    {
        log_error("camera still format couldn't be set");
        goto error;
    }

    /* Ensure there are enough buffers to avoid dropping frames */
    if (still_port->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM) {
        still_port->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM;
    }

    // ==================================================

    /* Enable component */
    status = mmal_component_enable(camera);
    if (status != MMAL_SUCCESS)
    {
        log_error("camera component couldn't be enabled");
        goto error;
    }

    rpi_camera_control_set_all_parameters(camera, &rpi_camera->params);

    rpi_camera->camera = camera;

    return status;

error:
    if (camera) { mmal_component_destroy(camera); }
    return status;
}

int rpi_camera_init(rpi_camera_t *rpi_camera, int width, int height)
{
    rpi_camera_param_default(&rpi_camera->params, width, height);

    MMAL_STATUS_T status = create_camera_component(rpi_camera);

    return (status == MMAL_SUCCESS) ? 0 : -1;
}

void rpi_camera_uninit(rpi_camera_t *rpi_camera)
{
    mmal_component_disable(rpi_camera->camera);
    mmal_component_destroy(rpi_camera->camera);
    return;
}

// ============================================================

/// Layer that preview window should be displayed on
#define PREVIEW_LAYER      2

typedef struct rpi_preview
{
    MMAL_RECT_T previewWindow;
    MMAL_COMPONENT_T *preview;
} rpi_preview_t;

int rpi_preview_init(rpi_preview_t *rpi_preview, const MMAL_RECT_T *previewWindow)
{
    MMAL_COMPONENT_T *preview = 0;
    MMAL_PORT_T *preview_port = NULL;
    MMAL_STATUS_T status;

    status = mmal_component_create(MMAL_COMPONENT_DEFAULT_VIDEO_RENDERER, &preview);
    if (status != MMAL_SUCCESS)
    {
        log_error("Unable to create preview component");
        goto error;
    }
    if (!preview->input_num)
    {
        status = MMAL_ENOSYS;
        log_error("No input ports found on component");
        goto error;
    }

    preview_port = preview->input[0];

    MMAL_DISPLAYREGION_T param;
    param.hdr.id = MMAL_PARAMETER_DISPLAYREGION;
    param.hdr.size = sizeof(MMAL_DISPLAYREGION_T);

    param.set = MMAL_DISPLAY_SET_LAYER;
    param.layer = PREVIEW_LAYER;

    param.set |= MMAL_DISPLAY_SET_ALPHA;
    param.alpha = 255;

    param.set |= (MMAL_DISPLAY_SET_DEST_RECT | MMAL_DISPLAY_SET_FULLSCREEN);
    param.fullscreen = 0;
    param.dest_rect = *previewWindow;

    status = mmal_port_parameter_set(preview_port, &param.hdr);
    if (status != MMAL_SUCCESS && status != MMAL_ENOSYS)
    {
        log_error("unable to set preview port parameters (%u)", status);
        goto error;
    }

    /* Enable component */
    status = mmal_component_enable(preview);
    if (status != MMAL_SUCCESS)
    {
        log_error("Unable to enable preview/null sink component (%u)", status);
        goto error;
    }

    rpi_preview->previewWindow = *previewWindow;
    rpi_preview->preview = preview;
    return status;

error:
    if (preview) {
        mmal_component_destroy(preview);
    }
    return status;
}

void rpi_preview_uninit(rpi_preview_t *rpi_preview)
{
    mmal_component_disable(rpi_preview->preview);
    mmal_component_destroy(rpi_preview->preview);

    return;
}

// ============================================================

#include <stdio.h>

void preview()
{
    MMAL_STATUS_T status;
    int ret;
    rpi_camera_t camera;   
    rpi_preview_t preview;

    ret = rpi_camera_init(&camera, 640, 480);
    if (ret != 0) return;

    MMAL_RECT_T win = {40, 40, 640, 480};
    rpi_preview_init(&preview, &win);

    MMAL_CONNECTION_T *previw_connection = NULL;
    status =  mmal_connection_create(&previw_connection, camera.camera->output[0], preview.preview->input[0], MMAL_CONNECTION_FLAG_TUNNELLING | MMAL_CONNECTION_FLAG_ALLOCATION_ON_INPUT);
    mmal_connection_enable(previw_connection);

    getchar();

    mmal_connection_disable(previw_connection);
    mmal_connection_destroy(previw_connection);

    rpi_camera_uninit(&camera);
    rpi_preview_uninit(&preview);
}

int main(int argc, char *argv[])
{
    bcm_host_init();

    vcos_log_register("rpi_camera", VCOS_LOG_CATEGORY);

    preview();

    bcm_host_deinit();

    return 0;
}
